/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef _C_WIRELESS_CELL_DATA_H_
#define _C_WIRELESS_CELL_DATA_H_
/**
* WirelessMap
* 
* Implements a wireless RF map 
* The RF-measurements are set to a maps from different 
* Each ID (=WLAN server) is put on a separte layer
*/
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include "owndebug.h"
#include "WirelessPositionLog.h"
#include "TOccupancyGrid.h"

/**
* Data container for each cell in the map
*/
struct TWirelessCellData{
	public:
	float w; ///<weight of a measurement
	float rss; ///rss value
			
	TWirelessCellData(){
		w=0;rss=0;
	}
	
	TWirelessCellData(float _w, float _rss){
		w = _w; rss=_rss;
	}
	
	void updateValue(float rss_value, float weight){
		if(rss == 0){
			rss=rss_value; w=weight;
		}else{
			if(w+weight<=0) return;
			rss = (w*rss+rss_value*weight)/(w+weight);
			if(weight>w)w=weight;
		}
	}
};


class CWirelessMap{
	public:
		CWirelessMap(CWirelessPositionLog wLog,
								 std::string linemapname, float resolution);
		~CWirelessMap();
		
		void toRGB(uint64_t key, unsigned char *rgb);
		void getDim(int &_Nx, int &_Ny){_Nx=Nx;_Ny=Ny;}
		int size(){return Nx*Ny;}
		TOccupancyGrid *grid; ///Used as a helper (coordinate trasformations etc)
		/**
		* Calculates cost matrix using one wireless measurement
		**/
		void computeWirelessMeasurementCost(MaCI::Wireless::CWirelessData *wd);
		void costToRGB(unsigned char *rgb);
	private:
		std::map<uint64_t,TWirelessCellData *> wirelessMapData;
		
		
		int Nx; //<size of the map in pixels
		int Ny; //<size of the map in pixels
		float RSS_range;
		float RSS_weight;
		float *measurementCost;
		std::vector<float> minCostVector;
		std::vector<int> minCostIndexVector;
		
		void setWirelessMeasurement(TWirelessCellData *wdata,float x, float y, uint32_t rss);
		void setBlock(TWirelessCellData *wdata, int pix, int piy, float r, 
																								float resolution,float RSS);
		
		void checkCostVector(float cost, int index, int maxnum);
		void resetCost();
};

#endif

